# -*- coding: utf-8 -*-
"""
Created on Fri Mar  7 23:19:57 2014

@author: DTHHA
"""
import random

import time

class moveEmotion(object):
    def __init__(self):
        self.HeadYaw = 0;
        self.HeadPitch = 0;
        self.LHand = 0;
        self.RHand = 0;
        self.HeadTime = 0;
        self.HandTime = 0;
        self.LShoulderRoll= 0;
        self.LShoulderPitch = 0;
        self.LShoulderTime = 0;
        self.RShoulderRoll = 0;
        self.RShoulderPitch = 0
        self.RShoulderTime = 0;
        self.desiredJoints = 1;
        
    def getTime(self):
        return time.time();
        
    def getDifftime(self, t_start):
        return time.time() - t_start;
        
    def genRand(self, maxFloat):

      '''/* generate secret number between 0 and maxFloat: */'''
      return (random.random() * maxFloat);
    '''
    /*
      *NAO's head (H25, V4.0) can move between:
      * Pitch: 0.5149 (forward) to -0.672 (backward)
      * Yaw  : 2.0857 (leftside) to -2.0857 (rightside)
      *
      * Rules to map between (Valence, Arousal) and (HeadPitch, HeadYaw):
      *  - Valence positive: pitch backward (i.e. gaze up) and small yaw angle variation
      *  - Valence negative: pitch forward (i.e. gaze down) and big yaw angle variation
      *
      *  - Arousal  : inversely mapped with execution time of each designated head's movement,
      *  generally, the faster the movement is, the higher the arousal is.
      *
      *@param:
      *valence compris between -1 and 1
      * arousal compris between 0 and 1
      */
      
      '''
    def moveAffectiveHead(self, valence, arousal):

        self.HeadPitch = valence / -2.0;
        self.HeadTime = 6*(1 - arousal);#time between 1->6
        #HeadYaw = (1-valence)*2.0;
    
    
        if (valence < 0):
            self.HeadYaw = self.genRand(2) - 1.0;
        else:
            self.HeadYaw = self.genRand(1.0) - 0.5;
        
        #std::cout<<"(valence,HeadPitch):("<<valence<<","<<HeadPitch<<")"<<std::endl;
        return self.HeadTime;
    
    '''        
    /* NAO's hand can be completely open (1) to completely closed (0).
      *
      * Mapping rules between (valence, arousal) to nao's hand movement:
      *  - Valence : the greater the valence is, the more open the hand is.
      *
      *  - Arousal : inversely mapped with execution time of each designated hand's movement,
      *  generally, the faster the movement is, the higher the arousal is.
      *
      *
      *valence compris between -1 and 1
      * arousal compris between 0 and 1
      */
      
    '''  
    def moveAffectiveHand(self, valence, arousal):
    
        self.RHand = self.LHand = (valence + 1) / 2.0;
        self.HandTime = 2.5-arousal;#time between 0.5->2.5
        return int(self.HandTime);
        
    '''
    /*
      *nao's LShoulder (H25, V4.0) can move between:
      * Pitch : 2.0857 (back) to -2.0857 (front)
      * Roll  : 1.3265 (outward) to -0.3142 (inward)
      *
      * Mapping rule for LShoulder's movement and (valence, arousal):
      *  - Valence (-1, 1) => (1.5, 0) LShoulderPitch : the greater the valence is, the higher the arm is.
      *  - Arousal (0, 1)  => (-0.15,1.15) LShoulderYaw: the greater the arousal, the more ouverture the left arm makes.
      *  - Arousal (0,)  => execution time: the higher the arousal is, the faster the movement is.
      *
      *nao's RShoulder (H25, V4.0) can move between:
      * Pitch : 2.0857 (back) to -2.0857 (front)
      * Roll  : -1.3265 (outward) to 0.3142 (inward)
      *
      * Mapping rule for RShoulder's movement and (valence, arousal)
      *  - Valence (-1, 1) => (1.5, 0) RShoulderPitch : the greater the valence is, the higher the arm is.
      *  - Arousal (0, 1)  => (0.15, -1.5) RShoulderYaw : the greater the arousal is, the more ouverture the right arm makes
      *  - Arousal (0,1)  => execution time: the higher the arousal is, the faster the movement is.
      *
      */
    '''
    def moveAffectiveShoulder(self, valence, arousal):
        self.LShoulderRoll = arousal + (self.genRand(0.3) - 0.15);
        self.LShoulderPitch = 0.7*(1.25-valence) + (self.genRand(0.1) - 0.05);
        self.LShoulderTime = 2*(2.5 - arousal);#0.5->3.5 seconds
    
        self.RShoulderRoll = -arousal+ (self.genRand(0.3) - 0.15);
        self.RShoulderPitch = 0.7*(1.25-valence) + (self.genRand(0.1) - 0.05);
        self.RShoulderTime = self.LShoulderTime;#1.5->3.5 seconds
        return self.LShoulderTime;
    '''
    /*
      *
      * Execute the motion in speed portion mode, better to map between Arousal and Speed.
      *
      *
      */
      
    '''
    def moveRobotWithSpeed(self, motion, speedPortion):
    
    
        #Setup joints' names and angles
        jointsName = [];
        targetAngles = [];
        if (self.desiredJoints & 1):
            jointsName.append("HeadYaw");jointsName.append("HeadPitch");
            targetAngles.append(self.HeadYaw);targetAngles.append(self.HeadPitch);
        
    
        if (self.desiredJoints & 2):
            jointsName.append("LShoulderRoll");jointsName.append("LShoulderPitch");
            jointsName.append("RShoulderRoll");jointsName.append("RShoulderPitch");
            targetAngles.append(self.LShoulderRoll);targetAngles.append(self.LShoulderPitch);
            targetAngles.append(self.RShoulderRoll);targetAngles.append(self.RShoulderPitch);
        
    
        if (self.desiredJoints & 4):
            jointsName.append("LHand");jointsName.append("RHand");
            targetAngles.append(self.LHand);targetAngles.append(self.RHand);
        
    
    
    
        try:
    
            motion.angleInterpolationWithSpeed(jointsName, targetAngles, speedPortion);
    
        except Exception, e:
            print "Could not execute the angleInterpolationWithSpeed"
            print "Error was: ", e
        
        return 0;
    '''    
    //===============================================================
    /*
      * For debug only.
      *
      * Sadness  : valence = (-1, -0.5), arousal = (0, 0.25), speedPortion = 0.05
      *
      * Happiness: ++head, hand++ : valence = (0.3, 0.6), arousal = (0.3, 1.0), speedPortion = 0.12
      *            ++shoulder++   : valence = (-0.4, 0.5), arousal = (0.3, 1.0), speedPortion = 0.12
      *
      * Anger: ++head, hand++ : valence = (-1.0, 1.0), arousal = (0.3, 1.0), speedPortion = 0.6
      *        ++shoulder++   : valence = (-0.4, 0.5), arousal = (0.3, 1.0), speedPortion = 0.6
      *
      * Serenity: ++head++             : valence = (-0.3, 0.7), arousal = (0.0, 0.25), speedPortion = 0.1
      *           ++hand, shoulder++   : valence = (-0.5, 0.5), arousal = (0.0, 0.25), speedPortion = 0.1
      *
      */
    //===============================================================
    /*
      * Intensity is between (0, 1). The higher the intensity, the sadder the expression should be.
      * This can be expressed by the HeadPitch, higher Headpitch is higher intensity; and in motion speed,
      * the higher the intensity, the slower the motion is.
      */
      
    '''  
    def moveSadness(self, motion, intensity, maxTime):
    
    
        #SpeedPortion should be between (0.05, 0.1)
        #std::cout<<"Sad head"<<std::endl;
        if (intensity < 0):
            intensity = 0;
        
        if (intensity > 1.0):
            intensity = 1.0;
        
        speedPortion = 0.05 - intensity/50.0;
        t = 0;
        start = self.getTime();
        while (t < maxTime):
            self.moveAffectiveHead(-0.5+self.genRand(-intensity), self.genRand(0.25));
            self.moveAffectiveHand(-0.5+self.genRand(-0.5), self.genRand(0.25));
            self.moveAffectiveShoulder(-0.5+self.genRand(-0.5), self.genRand(0.25));
            self.moveRobotWithSpeed(motion, speedPortion);
            t = self.getDifftime(start);
            #std::cout<<t<<std::endl;
    
    '''    
    /*
      * Happiness: ++head, hand++ : valence = (0.3, 0.6), arousal = (0.3, 1.0), speedPortion = 0.12
      *            ++shoulder++   : valence = (-0.4, 0.5), arousal = (0.3, 1.0), speedPortion = 0.12
      *            intensity affects 30% of the speed of the motion. The higher the intensity, the faster the motion is.
      *
    */'''
    def moveHappiness(self, motion, intensity, maxTime):
      #  //std::cout<<"Happy head"<<std::endl;
      if (intensity < 0):
            intensity = 0;
        
      if (intensity > 1.0):
            intensity = 1.0;
      
      speedPortion = 0.1 + intensity/30.0;
      t = 0;
      start = self.getTime();
      while (t < maxTime):
            self.moveAffectiveHead(0.3+self.genRand(0.3), 0.3+self.genRand(0.7));
            self.moveAffectiveHand(0.3+self.genRand(0.3), 0.3+self.genRand(0.7));
            self.moveAffectiveShoulder(self.genRand(0.9) - 0.4, 0.3+self.genRand(0.7));
            self.moveRobotWithSpeed(motion, speedPortion);
            t = self.getDifftime(start);
            #//std::cout<<t<<std::endl;
        
    def moveAnger(self, motion, intensity, maxTime):
            #//std::cout<<"Anger head"<<std::endl;
        if (intensity < 0):
            intensity = 0;
        
        if (intensity > 1.0):
            intensity = 1.0;
    
        t = 0;
        speedPortion = 0.4 + self.genRand(intensity) * 0.3;
        start = self.getTime();
        while (t < maxTime):
            self.moveAffectiveHead(-1.0+self.genRand(2.0), 0.7+self.genRand(0.3));
            self.moveAffectiveHand(-1.0+self.genRand(2.0), 0.7+self.genRand(0.3));
            self.moveAffectiveShoulder(-1.0+self.genRand(2.0), 0.3+self.genRand(0.7));
            self.moveRobotWithSpeed(motion, speedPortion);
            t = self.getDifftime(start);
            #print start, t;
        
    def moveSerenity(self, motion, intensity, maxTime):
        if (intensity < 0):
            intensity = 0;
        
        if (intensity > 1.0):
            intensity = 1.0;
        
        t = 0;
        speedPortion = 0.1 - self.genRand(intensity) * 0.07;
        start = self.getTime();
        while (t < maxTime):
            self.moveAffectiveHead(self.genRand(1.0)-0.3, self.genRand(0.25));
            self.moveAffectiveHand( self.genRand(1.0)-0.5, self.genRand(0.25));
            self.moveAffectiveShoulder( self.genRand(1.0)-0.7, self.genRand(0.25));
            self.moveRobotWithSpeed(motion, speedPortion);
            t = self.getDifftime(start);
            #//std::cout<<"Time: "<<t<<std::endl;
        
    '''        
    /*
     * From rightmost to leftmost: bit 1: head, bit 2: shoulder, bit 3: hand
     * For example: joint = 1 means head only
     *              joint = 2 means shoulder only
     *              joint = 3 means head + shoulder
     *              joint = 4 means hand only
     *              joint = 5 means head + hand
     *              joint = 6 means shoulder + hand
     *              joint = 7 means haed + shoulder + hand
     */'''
    def setDesiredJoints(self, joint):
        if ((joint == 0) or (joint > 7)):
            print "Desired joints invalid. Valid joint value is between [1, 7]. Stop!";
            return 1;
        
        self.desiredJoints = joint;
        return 0;

    


        